
import com.ridgesoft.intellibrain.*;


public class ThreadManager implements Runnable {

    private final static ThreadManager instance = new ThreadManager();
    private static ThreadNode list[] = new ThreadNode[30];//no more than 30 nodes allowed

    public ThreadManager() {
            new Thread(instance).start();
       }
    

    public ThreadManager getInstance() {
        return instance;
    }

    public static void registerThread(Runnable r, int p) {
        Thread t = new Thread(r);
        int i = 0;
        for (ThreadNode n : list) {
            if (n == null) {
                break;
            }
            i++;
        }
        list[i] = new ThreadNode(t, p);
        t.start();
    }

    public void run() {
        //IntelliBrainDigitalIO cutoff = IntelliBrain.getDigitalIO(12);
        
        //cutoff.setDirection(true);
        //cutoff.set();
    	
        registerThread(new Thread(Movement.getInstance()), 9);
        registerThread(new Thread(Ultrasound.getInstance()), 7);
        
        //registerThread(new Thread(SteeringServo.getInstance()), 4);
        //registerThread(new Thread(Navigation.getInstance()), 3);
        long time = System.currentTimeMillis();
        while (time + 1500 > System.currentTimeMillis()) {
            Thread.yield();
        }
        //cutoff.clear();
        
        //registerThread(new RoboMagellan(), 5);
        
        Thread.currentThread().setPriority(Thread.MAX_PRIORITY);
        int i = 10;
        IntelliBrainDigitalIO dead = IntelliBrain.getDigitalIO(3);
        dead.enablePulseMeasurement(true);
        int lastRead = -1, read;
        while (true) {
//            try {
//                Thread.sleep(10);
//            } catch (Throwable t) {
//            }
            Thread.yield();
            read = dead.readPulseDuration();
            if (read > 1300 || read == -1) {
              //  cutoff.clear();
            } else {
               // cutoff.set();
            }
            //System.out.println("" + read);
            if (i == 10) {
                for (ThreadNode t : list) {
                    if (t != null) {
                        t.thread.setPriority(Thread.MIN_PRIORITY);
                    }
                }
            }
            for (ThreadNode t : list) {
                if (t != null) {
                    if (t.priority == i) {
                        t.thread.setPriority(Thread.MAX_PRIORITY);
                    }
                }
            }
            i--;
            if (i == 0) {
                i = 10;
            }
            Thread.yield();
        }
    }
}
